/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvWorld/TrajSrc/line_traj.cpp
 * @Description  : 实现了直线轨迹的创建和相关操作，包括轨迹的生成、进度设置、速度计算和姿态投影等功能。 
 *                 主要用于移动设备在直线路径上的运动规划。  
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 17:26:44
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include <math.h>
#include "trajectory.h" 
/////////////////////////////////////////////////////////////////////////////
//   Implementation of the class.
//
//   Trajectory creation function.
//
/**
 * @brief 创建线性轨迹
 *
 * 根据起点、终点、轮子移动方向和轨迹的起始与终止位置创建线性轨迹。
 *
 * @param start_point 起点
 * @param end_point   终点
 * @param wheel_move_dir 轮子移动方向
 * @param from        轨迹起始位置（占轨迹总长度的比例）
 * @param to          轨迹终止位置（占轨迹总长度的比例）
 */
void LineTrajectory::CreateTrajectory(const Point& start_point, const Point& end_point,
                                      MoveDir wheel_move_dir, float from, float to) 
{
    type_ = (short)TrajType::LINE_TRAJ;
    wheel_move_dir_ = wheel_move_dir;
    // 构造线对象
    line_ = Line(start_point, end_point);
    float length = line_.Length();
    if (from > length) from = length;
    if (to < 0 || to > length) to = length;
    from_ = from;
    to_ = to;
    range_ = std::fabs(to - from);
    steer_angle_ = Angle(0.0f);
    heading_angle_ = line_.SlantAngle();
    if (wheel_move_dir_ == MoveDirTag::Backward) {
        heading_angle_ = !heading_angle_;
    }
}

//
//   SetProgress: Set the progress variable to specify the current point.
//
/**
 * @brief 设置相对进度
 *
 * 根据给定的速率和当前长度，更新当前点位置。
 *
 * @param rate 速率，表示当前进度相对于总进度的比例
 * @param current_length 当前长度，表示从起点到当前点的距离
 */
void LineTrajectory::SetRelProgress(float rate, float current_length)
{
    if (current_length > range_) current_length = range_;
    progress_rate_ = rate;
    current_point_ = line_.TrajFun(from_ + current_length);
    DEBUG_OUT("line:set prog:fRate=" << rate << ",fCurLen=" << current_length);
}

//
//	Preprocessing the progress variable to specify the current point
//
// void CLineTraj::PreSetProgress(float fRate, float fCurLen)
//{
//	m_fProgRate = fRate;
//}
/**
 * @brief 获取轨迹的范围
 *
 * 返回当前轨迹对象的范围。该范围是通过调用基类Trajectory的GetRange方法获取的。
 *
 * @return 范围，以浮点数形式表示
 */
float LineTrajectory::GetRange()
{
    return Trajectory::GetRange();
}


//
//
//   VelocityFun: The vehicle's velocity generation function.
//
/**
 * @brief 获取线轨迹的速度函数
 *
 * 根据移动方向和前进速率设置线轨迹的速度。
 *
 * @return 返回线轨迹的速度对象。
 */
Velocity& LineTrajectory::VelocityFun() 
{
    velocity_.linear_ = (wheel_move_dir_ == MoveDirTag::Forward) ? progress_rate_ : -progress_rate_;
    velocity_.vel_x_ = velocity_.linear_;
    velocity_.vel_y_ = 0.0f;
    velocity_.angular_ = 0.0f;
    return velocity_;
}

/**
 * @brief 根据转向角度计算速度
 *
 * 根据传入的虚拟速度和转向方向，计算并返回对应的速度对象。
 *
 * @param virtual_velocity 虚拟速度
 * @param steer_direction 转向方向
 * @return 计算得到的速度对象
 */
Velocity LineTrajectory::VelocityForSteerAngle(const float& virtual_velocity, int& steer_direction) 
{
    Velocity vel;
    vel.linear_ = (wheel_move_dir_ == MoveDirTag::Forward) ? virtual_velocity : -virtual_velocity;
    vel.vel_x_ = vel.linear_;
    vel.vel_y_ = 0.0f;
    steer_direction = static_cast<int>(SteerDirReport::SteerDirAuto);
    return vel;
}

//
//   Get the deviation between the trajectory and the specified posture.
//
/**
 * @brief 将给定的姿态投影到直线轨迹上，并计算投影点的相关信息
 *
 * 将给定的姿态投影到直线轨迹上，并根据投影点的位置更新 ProjectData 结构体的相关字段。
 *
 * @param posture 姿态对象，包含位置和角度信息
 * @param data 投影数据结构体指针，用于存储投影点的相关信息
 * @param init_posture 是否初始化姿态，此参数在本函数中未使用
 *
 * @return 如果投影成功，则返回 true；否则返回 false
 */
bool LineTrajectory::ProjectPosture(Posture& posture, ProjectData* data, bool init_posture) {
    (void)(init_posture);
    Angle angle_start_to, angle_end_to;
    float length_start_to = 0.0f, length_end_to = 0.0f;
    bool ret = false;
    Angle angle_diff = posture.GetAngle() - heading_angle_;
    data->y_ = 0;
    data->curvature_ = 0;
    data->theta_ = angle_diff.radian_;
    data->tangent_ = heading_angle_.radian_;

    if (std::isnan(data->theta_)) {
        data->x_ = data->y_ = data->theta_ = 0.0f;
        DEBUG_ERROR_OUT("Error: LineTrajectory: theta is NaN");
        return false;
    }

    Point& point_actual = posture.GetPntObject();

    // 判断是否在起始节点附近
    if (line_.GetStartPoint() != point_actual) {
        Line line_start_to_actual(line_.GetStartPoint(), point_actual);
        length_start_to = line_start_to_actual.Length();
        angle_start_to = line_start_to_actual.SlantAngle() - line_.SlantAngle();
        // The distance is smaller than 5cm and ang is less than 10 degree
        if (length_start_to < MAX_DIS_NEAR_POINT && std::fabs(angle_diff.NormAngle2()) < std::fabs(Angle(10.0f, AngleMode::InDegree).NormAngle2())) {
            data->type_ = ProjectData::ProjectionType::type_inside;
            DEBUG_OUT("#1 line project: inside; near start point; length_start_to=" << length_start_to);
        }
    } else {
        // 点的坐标相同,角度小于10度,则,近似看成是在起始节点上
        if (std::fabs(line_.SlantAngle().Degree() - posture.GetAngle().Degree()) < 10.0f) {
            data->type_ = ProjectData::ProjectionType::type_inside;
            DEBUG_OUT("#2 line project: inside; location start point");
        } else {
            DEBUG_ERROR_OUT("#3 error: line project: location start point, but angleDiff is too big");
            return false;
        }
    }

    // 判断是否在结束节点上
    if (line_.GetEndPoint() != point_actual) {
        Line line_end_to_actual(line_.GetEndPoint(), point_actual);
        length_end_to = line_end_to_actual.Length();
        angle_end_to = !line_.SlantAngle() - line_end_to_actual.SlantAngle();
        if (length_end_to < MAX_DIS_NEAR_POINT && std::fabs(angle_diff.NormAngle2()) < std::fabs(Angle(10.0f, AngleMode::InDegree).NormAngle2())) {
            data->type_ = ProjectData::ProjectionType::type_inside;
            DEBUG_OUT("#4 line project: inside; near end point");
        }
    } else {
        if (std::fabs(line_.SlantAngle().Degree() - posture.GetAngle().Degree()) < 10.0f) {
            data->type_ = ProjectData::ProjectionType::type_inside;
            DEBUG_OUT("#5 line project: inside; location end point");
        } else {
            DEBUG_ERROR_OUT("#6 error: line project: location end point, but angleDiff is too big");
            return false;
        }
    }

    // 当点既不在起点也不在终点（或靠近这两个点）时
    // 按照逻辑，我们有这三个参数：fnen1（到起点的距离）、angStartTo（到终点的倾斜角度和理论差）和angEndTo
    // 计算左右偏差
    // pData->uType：投影类型：0-内（内，锐角三角形），1-外（开始）-靠近开始-钝角三角形，2-外（结束）-靠近结束-钝角三角形
    // 确定投影类型
    if (angle_start_to.Degree() > 90.0f && angle_start_to.Degree() < 270.0f) {
        data->type_ = ProjectData::ProjectionType::type_outside_start;
        data->x_ = -length_start_to * sin(angle_start_to);
        data->y_ = -std::fabs(length_start_to * cos(angle_start_to));
        data->relative_progress_ = 0;
        ret = false;
        DEBUG_ERROR_OUT("#7 error: line project: outside start, x=" << data->x_ << ", y=" << data->y_ << ", relProg=0");
    } else if (angle_end_to.Degree() > 90.0f && angle_end_to.Degree() < 270.0f) {
        data->type_ = ProjectData::ProjectionType::type_outside_end;
        data->x_ = -length_start_to * sin(angle_start_to);
        data->y_ = std::fabs(length_start_to * cos(angle_start_to)) - from_;
        data->relative_progress_ = data->y_;
        ret = false;
        DEBUG_ERROR_OUT("#8 error: line project: outside end, x=" << data->x_ << ", y=" << data->y_ << ", relProg=" << data->relative_progress_);
    } else {
        data->type_ = ProjectData::ProjectionType::type_inside;
        data->x_ = -length_start_to * sin(angle_start_to);
        data->relative_progress_ = length_start_to * cos(angle_start_to) - from_;
        data->y_ = data->relative_progress_;
        ret = true;
        DEBUG_ERROR_OUT("#9 ok: line project: inside, x=" << data->x_ << ", y=" << data->y_ << ", relProg=" << data->relative_progress_);
    }

    if (wheel_move_dir_ == MoveDirTag::Backward) {
        data->x_ = -data->x_;
    }
    return ret;
}

/**
 * @brief 计算头部朝向与直线切线的夹角
 *
 * 计算当前轨迹的头部朝向与直线切线的夹角，并返回该夹角。
 *
 * @return 返回计算得到的夹角。
 */
Angle LineTrajectory::HeadToTangent() 
{
    Angle angle = heading_angle_ - line_.SlantAngle();
    DEBUG_OUT("LineTrajectory: HeadToTangent: heading_angle(degree)=" << heading_angle_.Degree()
              << ", tangentAngle(degree)=" << line_.SlantAngle().Degree()
              << ", angle(degree)=" << angle.Degree());
    return angle;
}
